'''
Author: your name
Date: 2021-07-25 21:24:38
LastEditTime: 2021-07-28 10:48:06
LastEditors: your name
Description: In User Settings Edit
FilePath: \MyOpenmv\color_and_line_find.py
'''
import sensor, image, time

sensor.reset()
sensor.set_pixformat(sensor.GRAYSCALE)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 2000)
clock = time.clock()
right_angle_threshold = (80,100)
THRESHOLD = (0,60)
def calculate_intersection(line1, line2):
    # 计算两条线的交点
    a1 = line1.y2() - line1.y1()
    b1 = line1.x1() - line1.x2()
    c1 = line1.x2()*line1.y1() - line1.x1()*line1.y2()

    a2 = line2.y2() - line2.y1()
    b2 = line2.x1() - line2.x2()
    c2 = line2.x2() * line2.y1() - line2.x1()*line2.y2()

    if (a1 * b2 - a2 * b1) != 0 and (a2 * b1 - a1 * b2) != 0:
        cross_x = int((b1*c2-b2*c1)/(a1*b2-a2*b1))
        cross_y = int((c1*a2-c2*a1)/(a1*b2-a2*b1))
        return (cross_x, cross_y)
    return (-1, -1)

def calculate_angle(line1, line2):
    # 利用四边形的角公式， 计算出直线夹角
    angle  = (180 - abs(line1.theta() - line2.theta()))
    if angle > 90:
        angle = 180 - angle
    return angle


def is_right_angle(line1, line2):
    global right_angle_threshold
    # 判断两个直线之间的夹角是否为直角
    angle = calculate_angle(line1, line2)

    if angle >= right_angle_threshold[0] and angle <=  right_angle_threshold[1]:
        # 判断在阈值范围内
        return True
    return False



border_threshold = ((0,0,160,120),(0,120,160,120),(160,0,160,120), (0,120,160,120) )
border_threshold = ((0,0,240,80),(240,0,80,160),(0,80,80,160),(80,160,240,80),(80,80,160,80))
i = 0
j = 0
def find_lines(img):

    for i in range(4):
        for j in range(i,4):

            line1 = img.get_regression([(255,255)],roi = border_threshold[i])


            line2 = img.get_regression([(255,255)],roi = border_threshold[j])

            if line1 and line2:
                if is_right_angle(line1, line2):
                    img.draw_line(line1.line(), color = (255,0,0))

                    img.draw_line(line2.line(), color = (255,0,0))
                    (cross_x, cross_y) = calculate_intersection(line1, line2)
                    return (cross_x, cross_y)

    return (None,None)





while(True):
    clock.tick()
    img = sensor.snapshot().binary([THRESHOLD])
    #img.draw_rectangle(border_threshold[0], color = (255, 0 ,0))
    #img.draw_rectangle(border_threshold[1], color = (255, 0 ,0))
    #img.draw_rectangle(border_threshold[2], color = (255, 0 ,0))
    #img.draw_rectangle(border_threshold[3], color = (255, 0 ,0))
    #img.draw_rectangle(border_threshold[4], color = (255, 0 ,0))
    find_lines(img)
    (cross_x, cross_y) = find_lines(img)




    if((cross_x, cross_y) == (None,None)):
        print("没有找到直线")

    else:
        img.draw_cross(cross_x, cross_y, color = (0,0,255))
        print(cross_x,cross_y)


